# 立创·庐山派-K230-CanMV开发板资料与相关扩展板软硬件资料官网全部开源
# 开发板官网：www.lckfb.com
# 技术支持常驻论坛，任何技术问题欢迎随时交流学习
# 立创论坛：www.jlc-bbs.com/lckfb
# 关注bilibili账号：【立创开发板】，掌握我们的最新动态！
# 不靠卖板赚钱，以培养中国工程师为己任

import time, os, sys
import utime
import machine
from media.sensor import *
from media.display import *
from media.media import *
from machine import UART
from machine import FPIOA

sensor_id = 2
sensor = None
sendbuf=bytearray(6)
sendbuf[0]=ord('b')
rect_flag=0
orgin_rect=(0,0,0,0)
# 显示模式选择：可以是 "VIRT"、"LCD" 或 "HDMI"
DISPLAY_MODE = "LCD"

# 根据模式设置显示宽高
if DISPLAY_MODE == "VIRT":
    # 虚拟显示器模式
    DISPLAY_WIDTH = ALIGN_UP(1920, 16)
    DISPLAY_HEIGHT = 1080
elif DISPLAY_MODE == "LCD":
    # 3.1寸屏幕模式
    DISPLAY_WIDTH = 800
    DISPLAY_HEIGHT = 480
elif DISPLAY_MODE == "HDMI":
    # HDMI扩展板模式
    DISPLAY_WIDTH = 1920
    DISPLAY_HEIGHT = 1080
else:
    raise ValueError("未知的 DISPLAY_MODE，请选择 'VIRT', 'LCD' 或 'HDMI'")

try:
    # 构造一个具有默认配置的摄像头对象
    sensor = Sensor(id=sensor_id)
    # 重置摄像头sensor
    sensor.reset()

    # 无需进行镜像翻转
    # 设置水平镜像
    sensor.set_hmirror(True)
    # 设置垂直翻转
    sensor.set_vflip(True)

    # 设置通道0的输出尺寸为显示分辨率
    sensor.set_framesize(width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, chn=CAM_CHN_ID_0)
    # 设置通道0的输出像素格式为RGB565
    sensor.set_pixformat(Sensor.RGB565, chn=CAM_CHN_ID_0)

    # 根据模式初始化显示器
    if DISPLAY_MODE == "VIRT":
        Display.init(Display.VIRT, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, fps=60)
    elif DISPLAY_MODE == "LCD":
        Display.init(Display.ST7701, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, to_ide=True)
    elif DISPLAY_MODE == "HDMI":
        Display.init(Display.LT9611, width=DISPLAY_WIDTH, height=DISPLAY_HEIGHT, to_ide=True)

    # 初始化媒体管理器
    MediaManager.init()

    # 启动传感器
    sensor.run()
    clock = utime.clock()
    color_threshold=[(255,255)]
    # 指定颜色阈值
    # 格式：[min_L, max_L, min_A, max_A, min_B, max_B]

    # 配置引脚
    fpioa = FPIOA()
    fpioa.set_function(11, FPIOA.UART2_TXD)
    fpioa.set_function(12, FPIOA.UART2_RXD)

    # 初始化UART2，波特率115200，8位数据位，无校验，1位停止位
    uart = UART(UART.UART3, baudrate=115200, bits=UART.EIGHTBITS, parity=UART.PARITY_NONE, stop=UART.STOPBITS_ONE)
    while True:
        os.exitpoint()
        clock.tick()

        # 捕获通道0的图像
        img = sensor.snapshot(chn=CAM_CHN_ID_0)
        gray_img=img.copy()

        gray_img.gamma_corr(15)
        gray_img=gray_img.to_grayscale()
        gray_img.binary([(240, 255)],invert=False)
#        #gray_img.erode(1)
        gray_img.dilate(3)
        blobs = gray_img.find_blobs(color_threshold,area_threshold = 5)

        if rect_flag == 0:
            rect_img=img.copy(0.5,0.5)
            rects = rect_img.find_rects(threshold=80000)
            if rects:
                rect_flag=1
                #检测矩形
                for rect in rects:
                    orgin_rect=(
                        int(rect.x() * 2),      # x 坐标
                        int(rect.y() * 2),      # y 坐标
                        int(rect.w() * 2),      # 宽度
                        int(rect.h() * 2)       # 高度
                    )
                    # 若想获取更详细的四个顶点，可使用 rect.corners()，该函数会返回一个有四个元祖的列表，每个元组代表矩形的四个顶点，从左上角开始，按照顺时针排序。
                    print(f"Rect {rect}")  # 打印线段信息

         #如果检测到颜色块
        if blobs:
            # 遍历每个检测到的颜色块
            for blob in blobs:
                # 绘制颜色块的外接矩形
                # blob[0:4] 表示颜色块的矩形框 [x, y, w, h]，
                img.draw_rectangle(blob[0:4],color=(255,255,255))

                # 在颜色块的中心绘制一个十字
                # blob[5] 和 blob[6] 分别是颜色块的中心坐标 (cx, cy)
                img.draw_cross(blob[5], blob[6])
                sendbuf[1]=ord('r')
                sendbuf[2]=(blob[5] >> 8) & 0xFF
                sendbuf[3]=blob[5]&0xFF
                sendbuf[4]=(blob[6] >> 8) & 0xFF
                sendbuf[5]=blob[6]&0xff
                uart.write(sendbuf)
                # 在控制台输出颜色块的中心坐标
                print("Blob Center: X={}, Y={}".format(blob[5], blob[6]))
        img.draw_rectangle(orgin_rect, color=(1, 147, 230), thickness=3)  # 绘制线段
        Display.show_image(img)
        print("fps = ", clock.fps())
except KeyboardInterrupt as e:
    print("用户停止: ", e)
except BaseException as e:
    print(f"异常: {e}")
finally:
    # 停止传感器运行
    if isinstance(sensor, Sensor):
        sensor.stop()
    # 反初始化显示模块
    Display.deinit()
    os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
    time.sleep_ms(100)
    # 释放媒体缓冲区
    MediaManager.deinit()
